/***********************************************************************************************//**
*\n  @file       commucationIF.c
*\n  @brief      uds诊断协议
*\n  @details
*\n -----------------------------------------------------------------------------------
*\n  文件说明：
*\n       1. uds诊断协议
*\n       
*\n -----------------------------------------------------------------------------------
*\n  版本:   	修改人:       修改日期:        	描述:
*\n  V0.01  项目组        2020.3.17          创建
***************************************************************************************************/

/**************************************************************************************************
* 头文件
***************************************************************************************************/
#include <stdint.h>
#include <stdio.h>
#include <stdlib.h>
#include <unistd.h>
#include <getopt.h>
#include <fcntl.h>
#include <string.h>
#include <libgen.h>
#include <sys/types.h>
#include <sys/stat.h>
#include <sys/file.h>
#include <sys/time.h> 
#include <sys/mman.h>
#include <errno.h>
#include <pthread.h>
#include <semaphore.h>

#include "fpga_io.h"
#include "spi_ctrl.h"
#include "commucationIF.h"

/**************************************************************************************************
* 宏定义、结构定义
***************************************************************************************************/
#define BUFFER_MAX_SIZE (2 * 1024)   ///< 最大数据长度
#define UDS_DEBUG   	1		     ///< 调试信息开关

/// 调试信息
#if UDS_DEBUG
#define PCL_NONE				 "\e[0m"
#define PCL_YELLOW				 "\e[1;33m"
#define PCL_RED 				 "\e[0;31m"

#define dbg_printf(...)   \
	do{ printf(PCL_NONE "[%s,%d,%s] ", __FILE__, __LINE__, __func__); \
		printf(__VA_ARGS__); }while(0)

#define dbg_info(buffer,size, ...)   \
	do{ 	printf(PCL_NONE "[%s,%d,%s] ", __FILE__, __LINE__, __func__); \
			printf(__VA_ARGS__); \
			char *p = (char*)buffer; \
			int ii_; \
			for (ii_=0; ii_<(int)size; ii_++) \
			{   printf("%02x ", *(p++)); if ((ii_ != 0) && (ii_ % 32 == 0)) printf("\n"); \
			} \
			if (ii_ % 32 != 0) printf("\n"); \
	}while(0)

#define dbg_warn(...)   \
		do{ printf(PCL_YELLOW "[warn,%s,%d,%s] ", __FILE__, __LINE__, __func__); \
		printf(__VA_ARGS__); }while(0)

#define dbg_error(...)   \
	do{ printf(PCL_RED "[error,%s,%d,%s] ", __FILE__, __LINE__, __func__); \
		printf(__VA_ARGS__); }while(0)

#define dbg_fatal(...)   \
	do{ printf(PCL_RED "[fatal,%s,%d,%s] ", __FILE__, __LINE__, __func__); \
		printf(__VA_ARGS__); }while(0)

#else
#define dbg_error(...)
#define dbg_fatal(...)
#define dbg_warn(...)
#define dbg_printf(...)
#define dbg_info(...) 
#endif

/**************************************************************************************************
* 全局变量声明、定义
***************************************************************************************************/
API int UdsEncoder(uint8_t txBuf[], int iID,  int iType, int dataSize);
API int UdsDecoder(uint8_t inBuf[], int inSize,
				int *pID, int *pType, uint8_t **pData,  int *pDateSize);

/**************************************************************************************************
* 私有变量声明、定义
***************************************************************************************************/
static int iInitFlags = 0;
static signal_t  tInputSignal1  = { -1, };
static output_signal_t tOutputSignal1 = { 0, };


/**************************************************************************************************
* 私有函数声明、定义
***************************************************************************************************/
static int RecvProcessCallback(uint8_t rxBuffer[], int iSize)
{

}


/*************************************************************************************//**
*\n 函数名称： UdsReadTask()
*\n 功能描述：
*\n            周期读取mcu的输入参数
*\n 输入参数：
*\n            void *arg  
*\n 输出参数: 
*\n            
*\n 返 回 值: 
*\n          
*\n -----------------------------------------------------------------------------------
*\n  版本:   修改人:       修改日期:        描述:
*\n  V0.01 罗先能        2020.3.25     创建
*****************************************************************************************/
static void *UdsReadTask(void *arg)
{
	int ret;
	static uint8_t RecvBuffer[BUFFER_MAX_SIZE];
	int iSize;
	int iType;
	int iId;
	uint8_t *pData;

	while(1)
	{
	loop1:
		usleep(50 * 1000);

		/// 1. 读取spi网络层数据
		iSize = 6 + 8;
		memset(RecvBuffer, 0, sizeof(RecvBuffer));
		ret = Layer3ReadFrame(RecvBuffer, sizeof(RecvBuffer), iSize);
		if (ret < 0)
		{
			dbg_error("Layer3ReadFrame() < 0\n");
			goto loop1;
		}
		
		/// 2. 解析uds协议
		ret = UdsDecoder(RecvBuffer, iSize, &iId, &iType, &pData, &iSize);
		if (ret < 0)
		{
			dbg_error("UdsDecoder() < 0\n");
			break;
		}
		dbg_info(pData, iSize, "id[%d],type[%d],data[%d]=", iId, iType, iSize);
		if (iId != 1)
		{
			dbg_error("ID  = %d\n", iId);
			break;
		}

		tInputSignal1.speed =  ((int) pData[1] << 8)  + (int) pData[0];
		tInputSignal1.ldw_on =  ((int) pData[2] >> 6)  &  0x3;
		tInputSignal1.gear  =  ((int) pData[2] >> 4)  &  0x3;
		tInputSignal1.brake =  ((int) pData[2] >> 2)  &  0x3;
		tInputSignal1.left_light  =  (int) pData[2]        &  0x1;
		tInputSignal1.right_light = ((int) pData[2] >> 1)  &  0x1;
		tInputSignal1.hmw_on =  ((int) pData[3] >> 4)  &  0x3;
		tInputSignal1.pcw_on =  ((int) pData[3] >> 2)  &  0x3;
		tInputSignal1.fcw_on =  ((int) pData[3] >> 0)  &  0x3;
		tInputSignal1.steering_angle = ((int) pData[5] << 8)  +  (int) pData[4];
		tInputSignal1.yaw_rate = (((int) pData[7] << 8)  +  (int) pData[6]) * 100;
	}

exit1:
	pthread_exit(0);
}


/*
* 功能: 向SPI模块设置输出信号
* 参数:	out 输入参数，输出信号信息
* 返回:  0  设置成功-1 设置失败
* 说明:
*/
static int spi_set_adas(output_signal_t *out)
{
	int ret;
	uint8_t txBuffer[BUFFER_MAX_SIZE];
	uint8_t *pData = NULL;
	int iSize;

	if (NULL == out)
		return -1;
	
	memcpy(&tOutputSignal1, out, sizeof(tOutputSignal1));
	
	memset(txBuffer, 0, sizeof(txBuffer));
	UdsEncoder(txBuffer,  2, 0, 260);   
	pData = & txBuffer[SIZE_OF_UDS_FRAME_HEAD];
	pData[1] = (tOutputSignal1.alarminfo.headway == -1) ? 0 : 
			   (tOutputSignal1.alarminfo.headway <<  1) | 1;
	pData[2] = 0;
	pData[3] = (tOutputSignal1.alarminfo.pcw_on & 0x3) << 4  |
			   (tOutputSignal1.alarminfo.ldw_on & 0x1);
	if (tOutputSignal1.alarminfo.ldw == 2)
	{		pData[3] |= 1 << 3;			}
	else if (tOutputSignal1.alarminfo.ldw == 1)
	{		pData[3] |= 1 << 2;			}
	pData[4] = (tOutputSignal1.alarminfo.car_on      & 0x1)      | 
			   	(tOutputSignal1.alarminfo.fcw_level  & 0x3) << 2 |
			   	(tOutputSignal1.alarminfo.hmw_level  & 0x3) << 4 |
			   	(tOutputSignal1.alarminfo.ufcw       & 0x1) << 6 |
			   	(tOutputSignal1.alarminfo.car_follow & 0x1) << 7 ;
	pData[5] =  (tOutputSignal1.alarminfo.ttc == -1) ? 
				0 : (((tOutputSignal1.alarminfo.ttc / 100)<<1) | 1);


	

	// 发送数据
	iSize = SIZE_OF_UDS_FRAME_HEAD + 260;
	ret = Layer3WriteFrame(txBuffer, iSize);
	if (ret < 0)
	{
		dbg_error("Layer3WriteFrame(txBuffer, iSize) < 0\n");
		return -1;
	}
	
	return 0;
}



/**************************************************************************************************
* 全局函数声明、定义

***************************************************************************************************/
/*************************************************************************************//**
*\n 函数名称： UdsDecoder()
*\n 功能描述：
*\n            解析UDS协议参数
*\n 输入参数：
*\n            uint8_t  inBuf[]  ： 网络层数据
*\n            int       inSize  ： 网络层数据长度
*\n 输出参数: 
*\n            uint8_t *pID      ： 任务ID
*\n            int     *pType    ： 任务属性 (0不需要应答，1需要应答)
*\n            uint8_t **pData   ： UDS数据指针（指向inBuf[]中的uds数据）
*\n            int * pDateSize   ： UDS长度
*\n            
*\n 返 回 值: 
*\n           0 成功; -1 失败; -n     错误码   
*\n -----------------------------------------------------------------------------------
*\n  版本:   修改人:       修改日期:        描述:
*\n  V0.01 罗先能        2020.3.24     创建
*****************************************************************************************/
API int UdsDecoder(uint8_t inBuf[], int inSize,
				int *pID, int *pType, uint8_t **pData,  int *pDateSize)
{
	if (NULL != pID)
	{
		*pID = (int) inBuf[2];
	}
	if (NULL != pType)
	{
		*pType = (int) inBuf[3];
	}
	if (NULL != pDateSize)
	{
		int iSize = (((int)inBuf[4])<<8) + (int)inBuf[5];
		if (iSize != inSize-6)
		{
			dbg_error("(iSize != %d\n", (inSize - 6));
			return -1;
		}
		*pDateSize = iSize;
	}
	if (NULL != pData)
	{
		*pData =  & inBuf[6];
	}
	
	return 0;
}

API int UdsEncoder(uint8_t txBuf[], int iID,  int iType, int dataSize)
{
	static int iSn;  

	++iSn;
	txBuf[0] = 0xff;
	txBuf[1] = 0xff;
	txBuf[2] = iID;
	txBuf[3] = iType;

	txBuf[4] = dataSize >> 8;
	txBuf[5] = dataSize;

 	return 0;
}

/*************************************************************************************//**
*\n 函数名称： spi_init()
*\n 功能描述：
*\n            初始化spi模块，只调用一次
*\n 输入参数：
*\n            udsrequest_t *udscbfunc  ： 回调函数列表
*\n 输出参数: 
*\n            
*\n 返 回 值: 
*\n           0 成功; -1 失败; -n     错误码   
*\n -----------------------------------------------------------------------------------
*\n  版本:   修改人:       修改日期:        描述:
*\n  V0.01 	罗先能        2020.2.17     创建
*****************************************************************************************/
API int spi_init(/* udsrequest_t *udscbfunc */)
{
	int ret = 0;

	if (iInitFlags == 0xaa)
		return 0;

	// 注册uds回调函数
	Layer3RegisterUdsProcess(RecvProcessCallback);

	memset(&tInputSignal1, -1, sizeof(tInputSignal1));
	memset(&tOutputSignal1, 0, sizeof(tOutputSignal1));

	static pthread_t read_tid = 0;
	ret = pthread_create(&read_tid, NULL, UdsReadTask, NULL);
	if(0 != ret)
	{
		dbg_error("pthread_create failed : %s\n", strerror(errno));
		return (-1);
	}
	dbg_printf("pthread_create(UdsRecvTask) ok\n");

	ret = Layer3Init();
	if(0 > ret)
	{
		dbg_error("Layer3Init() < 0 \n");
		return (-1);
	}
	
	iInitFlags = 0xaa;
	return 0;
}


/*************************************************************************************//**
*\n 函数名称： spi_uninit()
*\n 功能描述：
*\n            反初始化spi模块，只调用一次
*\n 输入参数：
*\n            void  
*\n 输出参数: 
*\n            
*\n 返 回 值: 
*\n           0 成功; -1 失败; -n     错误码   
*\n -----------------------------------------------------------------------------------
*\n  版本:   修改人:       修改日期:        描述:
*\n  V0.01 	罗先能        2020.2.17     创建
*****************************************************************************************/
API int spi_uninit(void)
{
	int ret = 0;

	ret = Layer3Uninit();
	if(0 > ret)
	{
		dbg_error("Layer3Uninit() < 0 \n");
		return (-1);
	}
	
	iInitFlags = 0;
	return 0;
}


/*
* 功能: 算法应用与底层通信数据交互接口
* 参数:[out]sig		报警信息、障碍物信息、车道线信息额等，由算法填充
		 [in]adas		车身输入信号
		 [in]dtcinfo	故障信息，故障发生对应的bit置1，未发生置0
		 				故障bit map标见udsdtc_t说明
		 [in]updatedid	用于did数据变化更新同步到MCU
		 				FPGA更新DID数据到MCU通过该参数实现
		 				eg:FPGA 软件版本号变化需要同步到MCU
* 返回:0	表示调用成功
		-1	err1
		-2	err2
* 说明:该接口由应用根据算法周期进行周期调用
*/
//API int spi_set_outputsignal(output_signal_t *adas /* , udsdtc_t *dtcinfo, udsdid_t *updatedid */)
API int spi_set_outputsignal(adas_out_t *ptAlarminfo, obj_lane_t *ptObjinfo)
{
	int ret;
	output_signal_t tAdas;
	
	// ret = spi_set_dtc(dtcinfo);	
	memset(&tAdas, 0, sizeof(output_signal_t));
	
	if (ptAlarminfo == NULL || ptObjinfo == NULL)
	{
		return -1;
	}
	
	memcpy(&tAdas.alarminfo, ptAlarminfo, sizeof(adas_out_t));
	memcpy(&tAdas.objinfo, ptObjinfo, sizeof(obj_lane_t));
	
	usleep(8*1000);
	ret = spi_set_adas(&tAdas);
	
	return ret;

#if 0	
	if (NULL != adas)
	{
		usleep(8*1000);
		ret = spi_set_adas(adas);
	}
#endif
}

/*
* 功能: 从SPI模块获取输入信号
* 参数:	in	输出参数，获取到的输入信号信息
* 返回:  0  获取成功, -1 获取失败
* 说明:
*/
API int spi_get_inputsignal(signal_t *in)
{
	memcpy(in, &tInputSignal1, sizeof(tInputSignal1));
	return 0;
}



API int spi_set_dtc(udsdtc_t *dtcinfo, udsdid_t *updatedid)
{
	int ret;
	uint8_t txBuffer[128];
	uint8_t *pData = NULL;
	int iSize;

	memset(txBuffer, 0, sizeof(txBuffer));
	UdsEncoder(txBuffer,  0x04, 0, 16);  

	pData = & txBuffer[SIZE_OF_UDS_FRAME_HEAD];
	memcpy(pData, dtcinfo, sizeof(udsdtc_t));
	
	// 发送数据
	iSize = SIZE_OF_UDS_FRAME_HEAD + 16;
	ret = Layer3WriteFrame(txBuffer, iSize);
	if (ret < 0)
	{
		dbg_error("Layer3WriteFrame(txBuffer, %d) < 0\n", iSize);
		return -1;
	}
	
	return 0;
}
